/*
 *  Copyright (C) 2004 Steve Harris, Uwe Koloska
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  $Id: example_server.c,v 1.2 2005/04/25 21:46:46 xovo Exp $
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>

#include "lo/lo.h"
#include "Vector3.h"

#define UPDATE_PATH "/infinity/update"
#define INFINITY_PORT "25669"

int updates = 0;

int min = 3686;
int max = 9164;

double floatweight = .3;
int checkpoint = 0;
int checkpoints = 0;

int directenabled = 0;
int pathenabled = 0;
int pathprogress = 0;

int difficulty = 0;

int debug = 0;

int posdebug = 0;
double floater = 0;
int fallcounter = 0;
int override = 1;
int done = 0;
int speed = 0;
int sunspeed = 0;
int forward = 0;
int backward = 0;
int up = 0;
int down = 0;
int left = 0;
int right = 0;
int forwardcommands = 0;
int backwardcommands = 0;
int upcommands = 0;
int downcommands = 0;
int leftcommands = 0;
int rightcommands = 0;

int mind = 0;
int stage = -1;

Vector3 position;
Vector3 target;
Vector3 storedtarget;

lo_address infinitydriver;

lo_address lightcontrol;

lo_address soundcontrol;

lo_address ipad;

void error(int num, const char *m, const char *path);

int update_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int direction_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int load_state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int mind_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int difficulty_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int override_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int debug_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int min_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int done_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int posdebug_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int position_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int cue_handler(const char *path, const char *types, lo_arg **argv, 
		    int argc, void* data, void *user_data);

int fail_handler(const char *path, const char *types, lo_arg **argv, 
		    int argc, void* data, void *user_data);

void * target_thread(void * arg);

void * target_set_thread(void * arg);

void * direction_reset_thread(void * arg);

void * target_move_thread(void * arg);

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

int main()
{
  target.x = 0;
  target.y = 0;
  target.z = 0;

  position.x = 0;
  position.y = 0;
  position.z = 0;

  infinitydriver = lo_address_new("192.168.2.89", INFINITY_PORT);

  lightcontrol = lo_address_new("192.168.2.70", "26000");

  soundcontrol = lo_address_new("192.168.2.64", "26000");

  ipad = lo_address_new("192.168.2.69", "1025");

  lo_server_thread st = lo_server_thread_new("25670", error);


  lo_server_thread_add_method(st, UPDATE_PATH, "i", update_handler, NULL);

  lo_server_thread_add_method(st, "/push", "f", direction_handler, NULL);

  lo_server_thread_add_method(st, "/stop", "f", emergency_handler, NULL);

  lo_server_thread_add_method(st, "/loadState", NULL, load_state_handler, NULL);

  lo_server_thread_add_method(st, "/playback", "i", playback_handler, NULL);

  lo_server_thread_add_method(st, "/mind", "i", mind_handler, NULL);

  lo_server_thread_add_method(st, "/difficulty", "f", difficulty_handler, NULL);

  lo_server_thread_add_method(st, "/override", "f", override_handler, NULL);

  lo_server_thread_add_method(st, "/min", "f", min_handler, NULL);

  lo_server_thread_add_method(st, "/debug", "f", debug_handler, NULL);

  lo_server_thread_add_method(st, "/done", "f", done_handler, NULL);

  lo_server_thread_add_method(st, "/posdebug", "f", posdebug_handler, NULL);

  lo_server_thread_add_method(st, "/position", "f", position_handler, NULL);

  lo_server_thread_add_method(st, "/cue", "f", cue_handler, NULL);

  lo_server_thread_add_method(st, "/fail", "f", fail_handler, NULL);

  lo_server_thread_start(st);

	pthread_t tid[4];
	int rc;

	rc = pthread_create( &tid[0], NULL, target_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[2], NULL, target_set_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[3], NULL, target_move_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}

  while (1) {
    pthread_mutex_lock(&mutex);
    if(updates == 0){
      printf("NO UPDATES!\n");
    }
    else if(target.x == 0 && target.y == 0 && target.z == 0){
      target.x = position.x;
      target.y = position.y;
      target.z = position.z;
    }
    updates = 0;
    printf("Position X:%f Y:%f Z:%f SPEED: %d\n", position.x, position.y, position.z, speed);
    printf("Target X:%f Y:%f Z:%f SPEED: %d MIND: %d\n", target.x, target.y, target.z, speed, mind);
    printf("SunPosition %f %f %f\n", sunposition.x, sunposition.y, sunposition.z);
    pthread_mutex_unlock(&mutex);
		usleep(1000000);
  }
	pthread_join( tid[0], NULL );

	pthread_mutex_destroy(&mutex);
  return 0;
}

void * target_thread(void * arg){
  while(1){
    pthread_mutex_lock(&mutex);
    lo_message message = lo_message_new();
    lo_message_add(message, "ii", (int)target.y, speed);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, "/infinity/target", message);
    lo_send_bundle(infinitydriver, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);

    lo_send(infinitydriver, "/infinity/suntarget", "fffi", suntarget.x, suntarget.y, suntarget.z, sunspeed);

    pthread_mutex_unlock(&mutex);
    usleep(50000);
  }
}

/* catch any incoming messages and display them. returning 1 means that the
 * message has not been fully handled and the server should try other methods */
int update_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data)
{
  pthread_mutex_lock(&mutex);
  int i;
  updates ++;
  if(posdebug == 0){
    position.y = argv[0]->i;
  }
  int temp = ((float)position.y)/(max-min)*100;
  int temp2 = ((float)position.y)/(max-min)*1000;
  if(temp > 100)
    temp = 100;
  if(temp2 > 1000)
    temp2 = 1000;
  if(temp < 0)
    temp = 0;
  if(temp2 < 0)
    temp2 = 0;
  lo_send(ipad, "/done", "f", (float)done);
  if(override != 1){
  	lo_send(soundcontrol, "/position", "i", temp2);
	lo_send(lightcontrol, "/position", "i", temp2);
  }
  if(done != 1){
	if(temp2 > 50 && checkpoints == 0){
		checkpoint = position;
		checkpoints = 1;
	}
  }
  if(temp2 > 250 && checkpoints == 1){
	checkpoint = position;
	checkpoints = 2;
  }
  if(temp2 > 550 && checkpoints == 2){
    checkpoint = position;
	checkpoints = 3;
  }
  if(temp2 > 950 && checkpoints == 3){
	checkpoint = max;
	checkpoints = 4;
  }
  
  else
  {
	lo_send(lightcontrol, "/position", "i", 0);
	lo_send(soundcontrol, "/position", "i", 0);
  }
  lo_send(ipad, "/position", "f", (float)temp);
  lo_send(ipad, "/mindpad", "f", (float)mind);
  for(i = 0; i < 6; i++){
    if(stage == i)
      lo_send(lightcontrol, "/stage", "ii", i, 1);
    else
      lo_send(lightcontrol, "/stage", "ii", i, 0);
  }
  lo_send(ipad, "/difficulty", "f", (float)difficulty);
  lo_send(ipad, "/override", "f", (float)override);
  lo_send(ipad, "/floater", "f", (float)floater);
  lo_send(ipad, "/debug", "f", (float)debug);
  lo_send(ipad, "/fallcount", "f", (float)fallcounter);
  lo_send(ipad, "/posdebug", "f", (float)posdebug);
  char buffer[10];
  sprintf(buffer, "%d", mind);
  lo_send(ipad, "/mindtext", "s", buffer);
  sprintf(buffer, "%d", temp);
  lo_send(ipad, "/positiontext", "s", buffer);
  sprintf(buffer, "%d", difficulty);
  lo_send(ipad, "/difficultytext", "s", buffer);
  pthread_mutex_unlock(&mutex);

  return 1;
}

int direction_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data)
{
  pthread_mutex_lock(&mutex);
  switch((int)argv[0]->f){
  case 0:
    up = 0;
    break;
  case 1:
    up = 1;
    break;
  case 2:
    down = 0;
    break;
  case 3:
    down = 1;
    break;
  }
  pthread_mutex_unlock(&mutex);

}

void * target_set_thread(void * arg){
  Vector3 temp;
  while(1){
    pthread_mutex_lock(&mutex);
    if(!directenabled && !pathenabled){
      temp.x = 0;
      temp.y = 0;
      temp.z = 0;
      if(override == 1){
        temp.x += forward;
        temp.x -= backward;
        temp.y -= down;
        temp.y += up;
        temp.z += right;
        temp.z -= left;
        floater = 0;
        fallcounter = 0;
        speed = 80;
		checkpoint = min;
		checkpoints = 0;
		done = 0;
      }
      if(override == 0){
        if(mind > difficulty){
			if(floater < 0 && fallcounter < 200)
				floater = 0;
			if(floater == 0){
				fallcounter ++;
				if(fallcounter > 200)
					fallcounter = 200;
			}
			if(floater != 0 && floater > -100){
				floater -= floatweight;
			}
			if(floater == 0 && fallcounter == 200){
				floater -= floatweight;
			}
        }
        if(mind < difficulty){
			if(floater < 100){
				floater += floatweight;
			}
			if(floater < 0){
				floater += 2.0*floatweight;
			}
        }
        if(mind == difficulty){
          if(floater > 0){
            floater -= floatweight;
          }
        }
        if(floater > 0){
          fallcounter = 0;
        }
      }
      if(floater > 0){
        temp.y += 1;
      }
      else if(floater < 0){
        temp.y -= 1;
      }
      int speedcap = 0;
      if(mind < difficulty && difficulty !=0){
        speedcap = (int)((((float)difficulty - (float)mind) / (float)difficulty) * 100.0);
      }
      else{
        speedcap = 0;
      }
      if(speedcap < 0)
        speedcap = 0;
      if(floater > speedcap){
        if(mind < difficulty){
          floater -= 2.0 * floatweight;
          if(floater < 0){
            floater = 0;
          }
        }
      }
      if(override == 0){
        speed = abs(floater) * .5;
		if(checkpoint == 3){
			speed = speed * .66;
		}
      }
      if(done == 1  && override == 0){
        temp.y = -1;
        speed = 80;
        floater = 0;
        fallcounter = 0;
		checkpoint = min;
		checkpoints = 0;
      }
      if(vecLength(temp) != 0)
        temp = vecDiv(temp, vecLength(temp));
      temp = vecMult(temp, 10000);
      target = vecAdd(position, temp);
	  if(override == 0){
		if(target.y < checkpoint){
			target.y = checkpoint;
			speed = .33;
		}
		if(target.y < min){
			target.y = min;
			speed = .33;
		}
	  }
    }
    pthread_mutex_unlock(&mutex);
    usleep(50000);
  }
}

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  while(1){
    printf("EMERGENCY STOP!\n");
    lo_message message = lo_message_new();
    lo_message_add(message, NULL);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, "/stop", message);
    lo_send_bundle(infinitydriver, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
    lo_send(lightcontrol, "/stop", "i", 1);
    usleep(50000);
  }
}

int direct_handler(const char *path, const char *types, lo_arg **argv, int argc,
  void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  suntarget.x = 2000+((argv[1]->f) * (10000-2000));
  suntarget.y = 10000-((argv[0]->f) * (10000-1000));
  suntarget.z = 0;
  pthread_mutex_unlock(&mutex);
}

int load_state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  printf("LOADING!\n");
  pthread_mutex_lock(&mutex);
  lo_message message = lo_message_new();
  lo_message_add(message, "i", 14100);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/state", message);
  lo_send_bundle(infinitydriver, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
  pthread_mutex_unlock(&mutex);
}

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  lo_message message = lo_message_new();
  lo_message_add(message, "i", argv[0]->i);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/playback", message);
  lo_send_bundle(infinitydriver, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
  pthread_mutex_unlock(&mutex);
}

int mind_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  if(debug == 0){
    mind = 100-(argv[0]->i);
  }
  //printf("MIND %f\n", mind);
  pthread_mutex_unlock(&mutex);
}

int difficulty_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  difficulty  = (int)argv[0]->f;
  //printf("GOT %f\n", difficulty);
  pthread_mutex_unlock(&mutex);
}

int override_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  override = (int)argv[0]->f;
  pthread_mutex_unlock(&mutex);
}

int min_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  lo_send(infinitydriver, "/min", "f", (float)argv[0]->f);
  pthread_mutex_unlock(&mutex);
}

int debug_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  debug = (int)argv[0]->f;
  pthread_mutex_unlock(&mutex);
}

int done_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  done = (int)((float)argv[0]->f);
  pthread_mutex_unlock(&mutex);
}

int posdebug_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  posdebug = (int)((float)argv[0]->f);
  pthread_mutex_unlock(&mutex);
}

int position_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  if(posdebug == 1){
    position.y = (int)((((float)argv[0]->f)/100) * (float)(max - min));
  }
  pthread_mutex_unlock(&mutex);
}

int cue_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  //printf("Cue\n");
  stage = (int)argv[0]->f;
  pthread_mutex_unlock(&mutex); 
}

int fail_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
	lo_send(lightcontrol, "/fail", "i", 1);
}
